#include "Protocol.h"
#include <cstring>
#include <cstdio>
#include <unistd.h>

#ifndef NO_QT
#include <QtDebug>
#endif

int protocol_make_pkt(TPacket& packet, uint8_t type, uint8_t status, uint8_t *data, uint32_t length)
{
	packet.header.head = 0;
	packet.header.head |= (type & P_TYPE_MASK);
	packet.header.head |= ((status << 5) & P_STAT_MASK);
	packet.header.genId = 0;
	packet.header.reserved = 0;
	packet.data = data;
	packet.length = length;
	return 0;
}

#ifndef NO_QT
int protocol_send_pkt(QTcpSocket *skt, TPacket& packet)
{
	static char buffer[READ_BUFFER_SIZE];
	int count = sizeof(TPacketHeader);

	memcpy(buffer, (uint8_t *)&packet.header, count);
	if(packet.data != NULL){
		memcpy(buffer + count, packet.data, packet.length);
		count += packet.length;
	}

	if(skt->write(buffer, count) != count){
//		qDebug() << "write < count";
		return -1;
	}
//	qDebug() << "protocol_send_pkt: count = " << count;
	return 0;
}
#else
int protocol_send_pkt(int skt, TPacket& packet)
{
	static unsigned char buffer[READ_BUFFER_SIZE];
	int count = sizeof(TPacketHeader);

	memcpy(buffer, (uint8_t *)&packet.header, count);
	if(packet.data != NULL){
		memcpy(buffer + count, packet.data, packet.length);
		count += packet.length;
	}

	if(write(skt, buffer, count) != count){
		perror("write");
		return -1;
	}
	return 0;
}

#endif

int protocol_get_status(const TPacket& packet)
{
	return (((uint8_t)packet.header.head) & P_STAT_MASK) >> 5;
}

// not used
#ifndef NO_QT
int protocol_talk(QTcpSocket *skt, TPacket& say, TPacket& hear)
{
	static char buffer[READ_BUFFER_SIZE];

	protocol_send_pkt(skt, say);

	int count;
//	qDebug() << "talk: packet sent, wait for read";
	if(skt->waitForReadyRead(-1) == false){
//		qDebug() << "wait return -1";
		return -1;
	}
	if((count = skt->read(buffer, READ_BUFFER_SIZE)) < (int)sizeof(TPacketHeader)){
//		qDebug() << "read return < count " << count;
		return -1;
	}
//	qDebug() << "talk: after read, count = " << count;
	hear.header = *(reinterpret_cast<TPacketHeader *>(buffer));
	if(count > (int)sizeof(TPacketHeader)){
		hear.data = (uint8_t *)buffer + sizeof(TPacketHeader);
		hear.length = count - sizeof(TPacketHeader);
	} else {
		hear.data = NULL;
		hear.length = 0;
	}
	return 0;
}

int protocol_hear(QTcpSocket *, TPacket& , TPacket& )
{
	// to be implemented
	return 0;
}
#endif
